/*
 * xbee.c
 *
 *  Created on: 01/06/2012
 *      Author: julian
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/select.h>
#include <sys/msg.h>
#include <errno.h>

#include "xbee_driver.h"
#include "xbee_driver_def.h"
#include "packet_def.h"
#include "packet_input.h"

int sendPacket_16(int serial_fd, char * payload, uint8_t size,
        uint8_t api_type, unsigned int addr, int frameId) {

    int checksum = 0x00;
    int lenght = 0;
    int i;
    int n = size + 9;
    uint8_t data[n];

    lenght = size + 5; //Lenght payload + 5

    data[0] = API_DELIMITER; //frame delimiter
    data[1] = 0x00; //MSB
    data[2] = lenght & 0xff; //LSB

    data[3] = api_type; //0x01 tx request API type
    checksum += data[3];

    data[4] = frameId; //frame id byte
    checksum += data[4];

    data[5] = (addr >> 8) & 0xff; //dest address MSB . LSB
    data[6] = (addr) & 0xff; //dest address LSB

    checksum += data[5];
    checksum += data[6];

    data[7] = 0x00 & 0xff; //options byte
    checksum += data[7];

    for (i = 0; i < size; i++) { //Data
        data[8 + i] = (payload[i] & 0xff);
        checksum += data[8 + i];
    }

    data[n - 1] = (0xff - checksum);

    serial_send(serial_fd, data, sizeof(data)); //send

#ifdef XBEE_API_DEBUG
    printf ("\nPaquete enviado------> ");
    for (i=0; i<sizeof(data); i++) {
        printf("%X ", data[i]);
    }
#endif
    return 0;
}

int sendPacket_64(int serial_fd, char * payload, uint8_t size,
        uint8_t api_type, uint64_t addr64, int frameId) {

    int checksum = 0x00;
    int lenght = 0;
    int i;
    int n = size + 15;
    uint8_t data[n];

    lenght = size + 11; //Lenght payload + 5

    data[0] = API_DELIMITER; //frame delimiter
    data[1] = 0x00; //MSB
    data[2] = lenght & 0xff; //LSB

    data[3] = api_type; //0x01 tx request API type
    checksum += data[3];

    data[4] = frameId; //frame id byte
    checksum += data[4];

    for (i = 0; i < 8; i++) { // 64bit addressing
        data[5 + i] = (addr64 >> (56 - (8 * i))) & 0xff;
        checksum += data[5 + i];
    }
    data[13] = 0x00 & 0xff; //options byte
    checksum += data[13];

    //printf("data: ");
    for (i = 0; i < size; i++) { //Data
        data[14 + i] = (payload[i] & 0xff);
        checksum += data[14 + i];
    }

    data[n - 1] = (0xff - checksum);

    serial_send(serial_fd, data, sizeof(data)); //send
#ifdef XBEE_API_DEBUG
    printf("\nPaquete enviado------> ");
    for (i = 0; i < sizeof(data); i++) {
        printf("%X ", data[i]);
    }
#endif
    return 0;
}

int ATCommand(int serial_fd, char * command, int frameId, int size) {

    int checksum = 0x00;
    int lenght = 0;
    int i;
    int n = size + 6;
    uint8_t data[n];

    lenght = size + 2; //Length payload

    data[0] = API_DELIMITER; //frame delimiter
    data[1] = 0x00; //MSB
    data[2] = lenght & 0xff; //LSB

    data[3] = AT_COMMAND; //0x08 AT_COMMAND request
    checksum += data[3];

    data[4] = frameId; //frame id byte
    checksum += data[4];

    if (size == 2) { // is it a query?

        data[5] = command[0] & 0xff;
        data[6] = command[1] & 0xff;
        checksum += data[5];
        checksum += data[6];

    } else { //or is it a setting?

        for (i = 0; i < size; i++) { //Data
            data[5 + i] = (command[i] & 0xff);
            checksum += data[5 + i];
        }
    }

    data[n - 1] = (0xff - checksum);

    serial_send(serial_fd, data, sizeof(data)); //send

#ifdef XBEE_API_DEBUG
    printf("Paquete AT enviado------> ");
    for (i = 0; i < sizeof(data); i++) {
        printf("%X ", data[i]);
    }
#endif

    return 0;
}

int remoteATCommand(int serial_fd, char * command, uint8_t size,
        uint8_t frameId, uint16_t addr16, uint64_t addr64, uint8_t opts) {

    int checksum = 0x00;
    int lenght = 0;
    int i;
    int n = size + 17;
    uint8_t data[n];

    lenght = size + 13; //Lenght payload

    data[0] = API_DELIMITER; //frame delimiter
    data[1] = 0x00; //MSB
    data[2] = lenght & 0xff; //LSB

    data[3] = AT_COMMAND_REMOTE_REQUEST; //0x08 AT_COMMAND request
    checksum += data[3];

    data[4] = frameId; //frame id byte
    checksum += data[4];

    for (i = 5; i < 13; i++) { //64-bit address
        data[i] = (addr64 >> (56 - (8 * (i - 5)))) & 0xff;
        checksum += data[i];
    }
    data[13] = (addr16 >> 8) & 0xff; //dest address MSB . LSB
    data[14] = (addr16) & 0xff; //dest address LSB
    checksum += data[13];
    checksum += data[14];

    data[15] = opts; //0x02;   //options byte, apply immediately
    checksum += data[15];

    if (size == 2) { // is it a query?
        data[16] = command[0] & 0xff;
        data[17] = command[1] & 0xff;
        checksum += data[16];
        checksum += data[17];
    } else { //or is it a setting?
        for (i = 0; i < size; i++) { //Data
            data[16 + i] = (command[i] & 0xff);
            checksum += data[16 + i];
        }
    }

    data[n - 1] = (0xff - checksum);

    serial_send(serial_fd, data, sizeof(data)); //send

#ifdef XBEE_API_DEBUG
    printf("\nPaquete AT remoto enviado------> ");
    for (i = 0; i < sizeof(data); i++) {
        printf("%X ", data[i]);
    }
#endif

    return 0;
}

int xbeeDriverInit(const char *path, speed_t brate, struct xbee_device *dev) {

    //-- Open the serial port
    //-- The speed is configure at 9600 baud
    dev->serial_fd = serial_open(path, brate);

    //-- Error checking
    if (dev->serial_fd == -1) {
#ifdef XBEE_DRV_DEBUG
        perror("xbeeDriverInit:serial_open.Error abriendo el puerto serie");
#endif
        return -1;
    }

    dev->out_msqid = msgget((key_t) OUTPUT_QUEUE_KEY, 0644 | IPC_CREAT); // cola para almacenar tramas salientes
    if (dev->out_msqid == -1) {
#ifdef XBEE_DRV_DEBUG
        perror("xbeeDriverInit:msgget.Error creando la cola de salida");
#endif
        return -1;
    }

    dev->in_msqid = msgget((key_t) INPUT_QUEUE_KEY, 0644 | IPC_CREAT); // cola para almacenar tramas entrantes
    if (dev->in_msqid == -1) {
#ifdef XBEE_DRV_DEBUG
        perror("xbeeDriverInit:msgget.Error creando la cola de entrada");
#endif
        return -1;
    }

    dev->good = 0;
    dev->acked = 0;
    dev->rbuf = (uint8_t*) malloc(404 * sizeof(uint8_t));
    if (dev->rbuf == NULL) {
#ifdef XBEE_DRV_DEBUG
        perror("xbeeDriverInit:malloc.Error al apartar el buffer de entrada");
#endif
        return -1;
    } else {
        dev->rbuf = memset(dev->rbuf, 0x00, 404);
        return 0;
    }
}

int receiveFrames(struct xbee_device *mod, int timeout_usec, int timeout_sec) {

    int j = 0, i = 0, k = 0;
    int n = 0;
    fd_set fds;
    struct timeval timeout;
    struct msgbuf_rx qbuf;
    struct rx_frame trama;
    unsigned char *p;

    int verified = 0;

    FD_ZERO(&fds);

    if (mod->rbuf == NULL) {
        perror("mod.rbuf null");
        return -1;
    }

    timeout.tv_sec = timeout_sec;
    timeout.tv_usec = timeout_usec;
    FD_SET(mod->serial_fd, &fds);

    if (select(FD_SETSIZE, &fds, NULL, NULL, &timeout) > 0) {
        if ((n = serial_read(mod->serial_fd, mod->rbuf, MAX_FRAME_SIZE,
                SERIAL_READ_TIMEOUT_USEC)) > 0) {
#ifdef XBEE_DRV_DEBUG
            printf("\n n: %d |", n);
#endif

            for (i = 0; i < n; i++) {
                if (mod->rbuf[i] == 0x7E) {
                    j++;
                }
            }
            if (j == 0) {
                return -1;
            }
            uint8_t pck[j][MAX_FRAME_SIZE];
            j = 0;

            for (i = 0; i < n; i++) {
                if (mod->rbuf[i] != 0x7E) {
                    pck[j][k] = mod->rbuf[i];
                    pck[j][k + 1] = 0x0D;
                    ++k;
                } else if (i != 0) {
                    pck[j][k] = 0x0D;
                    k = 0;
                    ++j;
                }
            }

            k = 0;
            uint8_t checksum = 0;
            for (i = 0; i <= j; ++i) {
                while (pck[i][k] != 0x0D) {
                    if (k > 1) {
                        checksum += pck[i][k];
                    }
                    //printf("%X ",pck[i][k]);
                    k++;
                }
                if (checksum == 0xFF) {
                    verified++;
                    //printf("\npacket %d: ", i);
                    trama.length = pck[i][1];
                    trama.api_id = pck[i][2];
                    for (k = 0; k < trama.length - 1; ++k) {
                        trama.data[k] = pck[i][3 + k];
                        //printf("%X ", trama->data[k]);
                    }
                    p = &trama.data[4];
                    qbuf.mtype = trama.api_id;
                    qbuf.trama = trama;
                    if (trama.api_id == TX_STATUS_RESPONSE && trama.data[0] != 0
                            && trama.data[1] == 0) {
                        mod->acked += 1;
#ifdef XBEE_DRV_DEBUG
                        printf("\nsent: %d ", mod->acked);
#endif
                    } else if (trama.api_id != TX_STATUS_RESPONSE) {
                        mod->good += 1;
#ifdef XBEE_DRV_DEBUG
                        printf("\ndevice good: %d \n", mod->good);
#endif
                    }
                    if (msgsnd(mod->in_msqid, &qbuf,
                            sizeof(qbuf) - sizeof(long), IPC_NOWAIT) == -1) {
                        perror("receive_packets:msgsnd");
                    } else {
#ifdef XBEE_DRV_DEBUG
                        printf("\nmtype: %X, length: %d, api_id: %X ",
                                qbuf.mtype, qbuf.trama.length,
                                qbuf.trama.api_id);
                        printf("\ndata: ");
                        for (k = 0; k < trama.length - 1; ++k) {
                            printf("%X ", qbuf.trama.data[k]);
                        }
#endif
                    }
                } else {
                    trama.length = 0; // FLUSH PACKET
                    trama.api_id = 0;
                    memset(trama.data, 0x00, MAX_FRAME_SIZE);
                    //mod->rbuf=memset(mod->rbuf, 0x00, 404);
                    checksum = 0;
                    k = 0;
                    return 0;
                }
            }
        }
        return verified;
        n = 0;
    }
    return -1;
}

void sendFrames(struct xbee_device *mod, struct msgbuf_tx TX) {

    if (TX.mtype == TX_REQUEST_16) {
        sendPacket_16(mod->serial_fd, &TX.trama.data, TX.trama.length,
                TX.trama.api_id, TX.trama.addr_16, TX.trama.frameid);
    } else if (TX.mtype == MQ_TX_REQUEST_64) {
        sendPacket_64(mod->serial_fd, &TX.trama.data, TX.trama.length,
                TX.trama.api_id, TX.trama.addr_64, TX.trama.frameid);
    } else if (TX.mtype == AT_COMMAND_REMOTE_REQUEST) {
        remoteATCommand(mod->serial_fd, &TX.trama.data[1], TX.trama.length,
                TX.trama.frameid, TX.trama.addr_16, TX.trama.addr_64,
                TX.trama.data[0]);
    } else if (TX.mtype == AT_COMMAND) {
        ATCommand(mod->serial_fd, &TX.trama.data, TX.trama.frameid,
                TX.trama.length);
    }
}

int xbeeDriverExit(struct xbee_device * dev) {

    free(dev->rbuf);

    if (msgctl(dev->out_msqid, IPC_RMID, NULL) == -1) {
#ifdef XBEE_DRV_DEBUG
        perror("xbeeDriverExit:msgctl.Error al destruir la cola de salida");
#endif
        return -1;
    }
    if (msgctl(dev->in_msqid, IPC_RMID, NULL) == -1) {
#ifdef XBEE_DRV_DEBUG
        perror("xbeeDriverExit:msgctl.Error al destruir la cola de entrada");
#endif
        return -1;
    }

    serial_close(dev->serial_fd);

    return 0;
}
